/*
 * Capstone.c
 *
 * Created: 1/11/2013 5:30:11 PM
 *  Author: Alexander
 */

#include "Capstone.h"

/**************************************************************************
* Method:		main
* FullName:		main
* Access:		public 
* Returns:		int
* Qualifier:	
* Parameter:	void
**************************************************************************/
int main(void)
{	
	Motor_Adjust();
	AvrXSetKernelStack(0);
	
	AvrXSetSemaphore(&EEPromMutex);
	AVRX_HARDWARE_SETUP_COMMANDS;	
	
	stderr = stdout = stdin = &uart_stream; /* Redirecting the standard I/O */
		
	UARTInit(); /* Initial UART */

	
	Current_Angle_ptr = New_Vector();
	Previous_Vector_ptr = New_Vector();
	Angle_ptr = New_Vector();	
	
	AvrXRunTask(TCB(Gyro));
	AvrXRunTask(TCB(GyroAverage));
		
	Epilog();
	while(1);
}

/**************************************************************************
* Method:		Motor_Adjust
* FullName:		Motor_Adjust
* Access:		public 
* Returns:		void
* Qualifier:	
* Description:	Allows the motor driver to be adjusted so that the water bags will be adjusted correctly
**************************************************************************/
void Motor_Adjust()
{
	PORTB |= (1<<PB7);				// Pull up resistors enable
	PORTE |=  ((1<<PE3) | (1<<PE2));
	MotorInit();	
	//Down Button
	while(bit_is_set(PINB, PB7))
	{
		// Left Button
		if(bit_is_clear(PINE,PE3))
			MotorOnBack();
		//Right Button
		else if(bit_is_clear(PINE,PE2))
			MotorOnForward();
		else
			MotorOff();
	}
}

/**************************************************************************
* Method:		GyroAverage
* FullName:		GyroAverage
* Access:		public 
* Returns:		void
* Qualifier:	
* Parameter:	void
**************************************************************************/
void GyroAverage(void)
{	
	//int count;
	while(1)
	{	
		//count = Get_Count(Angle_ptr);
		
//		Scalar_Divide(&Angle_ptr, count);
		Scalar_Divide(Angle_ptr, 2);
		//Scalar_Multiply(&Angle_ptr, 2);
		Add_Vectors(Previous_Vector_ptr, Angle_ptr);
		
		BeginCritical();
		printf("X: %6d \nY: %6d \nZ: %6d\n\n", Previous_Vector_ptr->AXIS_X, Previous_Vector_ptr->AXIS_Y, Previous_Vector_ptr->AXIS_Z);
		Vector_Init(Angle_ptr);
		EndCritical();
		AvrXSetSemaphore(&turnServos);
		AvrXDelay(&Gyro_Average_TCB, 500);
	}
}

/**************************************************************************
* Method:		Gyro
* FullName:		Gyro
* Access:		public 
* Returns:		void
* Qualifier:	
* Parameter:	void
**************************************************************************/
void Gyro(void)
{	
	InitSPI();	 
	Gyro_Init();

	AxesVector_ptr zeroRate_ptr = Zero_Rate_Calc();
	 
	while(1)
	{
		BeginCritical();
		Calculate_Angle(zeroRate_ptr, Current_Angle_ptr, Angle_ptr);
		EndCritical();
		AvrXDelay(&Gyro_TCB, GyroDelay);
	}
}
 
 /**************************************************************************
 * Method:		Pump
 * FullName:		Pump
 * Access:		public 
 * Returns:		void
 * Qualifier:	
 * Parameter:	void
 * Description:	
 **************************************************************************/
 void Pump(void)
 {	 
	MotorInit();
	AvrXInitTask(TCB(Pump_Forward));
	AvrXInitTask(TCB(Pump_Backward));
	AvrXSetSemaphore(&Pump_Backward_Done);
	
	while(1)
	{
		AvrXResume(PID(Pump_Forward));
		AvrXResume(PID(Pump_Backward));
		AvrXYieldHack(2);
	}		
 }
 
 /**************************************************************************
 * Method:		Pump_Forward
 * FullName:		Pump_Forward
 * Access:		public 
 * Returns:		void
 * Qualifier:	
 * Description:	
 **************************************************************************/
 void Pump_Forward(void)
 {
	while(1)
	{
		AvrXWaitSemaphore(&Pump_Backward_Done);
		AvrXWaitSemaphore(&Pump_Forward_Mutex);
		MotorOnForward();
		AvrXDelay(&PumpTime_TCB, Divetime);
		MotorOff();
		AvrXSetSemaphore(&Pump_Forward_Done);
	}
 }
 
 /**************************************************************************
 * Method:		Pump_Backward
 * FullName:		Pump_Backward
 * Access:		public 
 * Returns:		void
 * Qualifier:	
 * Parameter:	void
 * Description:	
 **************************************************************************/
 void Pump_Backward(void)
 {

	 while(1)
	 {
		 AvrXWaitSemaphore(&Pump_Forward_Done);
		 AvrXWaitSemaphore(&Pump_Backward_Mutex);
		 MotorOnBack();
		 AvrXDelay(&PumpTime_TCB, Divetime);
		 MotorOff();
		 AvrXSetSemaphore(&Pump_Backward_Done);
	 }	 
 }
 
 /**************************************************************************
 * Method:		Pressure_Sensor
 * FullName:		Pressure_Sensor
 * Access:		public 
 * Returns:		void
 * Qualifier:	
 * Parameter:	void
 * Description:	Reads the pressure sensor data and determines whether or not
 *				the pump needs to adjust the water in the bags.
 **************************************************************************/
 void Pressure_Sensor(void)
 {
	 Pressure_Init();
	 TimerControlBlock Pressure_Read_Timer;
	 uint16_t pressure;
	 
	 while(1)
	 {
		Pressure_Read_Sensor(&pressure);
		if(pressure > MAX_PRESSURE)
			AvrXSetSemaphore(&Pump_Backward_Mutex);
		else if(pressure < MIN_PRESSURE)
			AvrXSetSemaphore(&Pump_Forward_Mutex);
		AvrXDelay(&Pressure_Read_Timer, 5);
	 }		 
 }

AVRX_SIGINT(AVRX_HARDWARE_TIMER_SIGNAL)
{
	IntProlog();
	AvrXTimerHandler();
	Epilog();
}
